/***************************************************************************************************************************
* command_to_rotors.h
*
* Author: Qyp
*                （Wxt移植的）
*
* Update Time: 2020.08.12
*
* Introduction:  Drone control command send class using Mavros package
*         1. Ref to the Mavros plugins (setpoint_raw, loca_position, imu and etc..)
*         2. Ref to the Offboard Flight task in PX4 code: https://github.com/PX4/Firmware/blob/master/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp
*         3. Ref to the Mavlink module in PX4 code: https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp
*         4. Ref to the position control module in PX4: https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control
*         5. Ref to the attitude control module in PX4: https://github.com/PX4/Firmware/blob/master/src/modules/mc_att_control
*         6. 还需要考虑复合形式的输出情况
*         7. 移植的，懒得修改上面的参考了（Wxt）
* 主要功能：
*    本库函数主要用于连接prometheus_control与mavros两个功能包。简单来讲，本代码提供飞控的状态量，用于控制或者监控，本代码接受控制指令，并将其发送至飞控。
* 1、发布prometheus_control功能包生成的控制量至mavros功能包，可发送期望位置、速度、角度、角速度、底层控制等。
* 2、订阅mavros功能包发布的飞控状态量（包括PX4中的期望位置、速度、角度、角速度、底层控制），用于检查飞控是否正确接收机载电脑的指令
* 3、解锁上锁、修改模式两个服务。

***************************************************************************************************************************/
#ifndef COMMAND_TO_ROTORS_H
#define COMMAND_TO_ROTORS_H

#include <ros/ros.h>
#include <bitset>
#include <math_utils.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/Imu.h>
#include <uav_drone_msgs/DroneState.h>
#include <uav_drone_msgs/AttitudeTarget_replace.h>
#include <uav_drone_msgs/PositionTarget_replace.h>
#include <uav_drone_msgs/AttitudeReference.h>
#include <eigen_conversions/eigen_msg.h>
using namespace std;

class command_to_rotors
{
    public:
    //constructed function 1
    command_to_rotors(void):
    command_nh("~")
    {
        command_nh.param<string>("mav_name", mav_name, "firefly");
        pos_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
        vel_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
        accel_drone_fcu_target  = Eigen::Vector3d(0.0,0.0,0.0);
        q_fcu_target            = Eigen::Quaterniond(0.0,0.0,0.0,0.0);
        euler_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
        rates_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
        Thrust_target           = 0.0;

        // 【订阅】无人机期望位置/速度/加速度 坐标系:ENU系
        //  本话题来自飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp读取), 对应Mavlink消息为POSITION_TARGET_LOCAL_NED, 对应的飞控中的uORB消息为vehicle_local_position_setpoint.msg
        position_target_sub = command_nh.subscribe<uav_drone_msgs::PositionTarget_replace>("/drone_pos_estimator/setpoint_raw/target_local", 10, &command_to_rotors::pos_target_cb,this);

        // 【订阅】无人机期望角度/角速度 坐标系:ENU系
        //  本话题来自飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp读取), 对应Mavlink消息为ATTITUDE_TARGET (#83), 对应的飞控中的uORB消息为vehicle_attitude_setpoint.msg
        attitude_target_sub = command_nh.subscribe<uav_drone_msgs::AttitudeTarget_replace>("/drone_pos_estimator/setpoint_raw/target_attitude", 10, &command_to_rotors::att_target_cb,this);

        // 【发布】位置环控制
        //  本话题发布位置环信息至rotors_simulator的lee_control_node,位置环包括两个部分
        //  pose/point：控制无人机的位置；pose/orientation可控制飞行的期望角度（需要在发布前将偏航角转换为四元数）
        setpoint_raw_local_pub = command_nh.advertise<geometry_msgs::PoseStamped>("/"+mav_name + "/command/pose", 10);

    }

		//constructed function 2
    command_to_rotors(string Mav_name):
        command_nh("")
    {
        mav_name = Mav_name;
        
        pos_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
        vel_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
        accel_drone_fcu_target  = Eigen::Vector3d(0.0,0.0,0.0);
        q_fcu_target            = Eigen::Quaterniond(0.0,0.0,0.0,0.0);
        euler_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
        rates_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
        Thrust_target           = 0.0;

        // 【订阅】无人机期望位置/速度/加速度 坐标系:ENU系
        //  本话题来自飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp读取), 对应Mavlink消息为POSITION_TARGET_LOCAL_NED, 对应的飞控中的uORB消息为vehicle_local_position_setpoint.msg
        position_target_sub = command_nh.subscribe<uav_drone_msgs::PositionTarget_replace>("/drone_pos_estimator/setpoint_raw/target_local", 10, &command_to_rotors::pos_target_cb,this);

        // 【订阅】无人机期望角度/角速度 坐标系:ENU系
        //  本话题来自飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp读取), 对应Mavlink消息为ATTITUDE_TARGET (#83), 对应的飞控中的uORB消息为vehicle_attitude_setpoint.msg
        attitude_target_sub = command_nh.subscribe<uav_drone_msgs::AttitudeTarget_replace>("/drone_pos_estimator/setpoint_raw/target_attitude", 10, &command_to_rotors::att_target_cb,this);

        // 【发布】位置/速度/加速度期望值 坐标系 ENU系
        //  本话题要发送至飞控(通过Mavros功能包 /plugins/setpoint_raw.cpp发送), 对应Mavlink消息为SET_POSITION_TARGET_LOCAL_NED (#84), 对应的飞控中的uORB消息为position_setpoint_triplet.msg
         setpoint_raw_local_pub = command_nh.advertise<geometry_msgs::PoseStamped>("/"+mav_name + "/command/pose", 10);


    }

    string mav_name;
    //Target pos of the drone [from fcu]
    Eigen::Vector3d pos_drone_fcu_target;
    //Target vel of the drone [from fcu]
    Eigen::Vector3d vel_drone_fcu_target;
    //Target accel of the drone [from fcu]
    Eigen::Vector3d accel_drone_fcu_target;
    //Target att of the drone [from fcu]
    Eigen::Quaterniond q_fcu_target;
    Eigen::Vector3d euler_fcu_target;
    Eigen::Vector3d rates_fcu_target;
    //Target thrust of the drone [from fcu]
    float Thrust_target;

    ros::ServiceClient arming_client;

    ros::ServiceClient set_mode_client;

    //Idle. Do nothing.
    void idle();

    // takeoff
    void takeoff();

    //  land
    void land();

    //发送位置期望值至飞控（输入：期望xyz,期望yaw）
    void send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp);



    
    private:

        ros::NodeHandle command_nh;

        ros::Subscriber position_target_sub;
        ros::Subscriber attitude_target_sub;
        ros::Publisher setpoint_raw_local_pub;

        void pos_target_cb(const uav_drone_msgs::PositionTarget_replace::ConstPtr& msg)
        {
            pos_drone_fcu_target = Eigen::Vector3d(msg->position.x, msg->position.y, msg->position.z);

            vel_drone_fcu_target = Eigen::Vector3d(msg->velocity.x, msg->velocity.y, msg->velocity.z);

            accel_drone_fcu_target = Eigen::Vector3d(msg->acceleration_or_force.x, msg->acceleration_or_force.y, msg->acceleration_or_force.z);
        }

        void att_target_cb(const uav_drone_msgs::AttitudeTarget_replace::ConstPtr& msg)
        {
            q_fcu_target = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);

            //Transform the Quaternion to euler Angles
            euler_fcu_target = quaternion_to_euler(q_fcu_target);

            rates_fcu_target = Eigen::Vector3d(msg->body_rate.x, msg->body_rate.y, msg->body_rate.z);

            Thrust_target = msg->thrust;
        }



};

void command_to_rotors::takeoff()
{
     geometry_msgs::PoseStamped pos_setpoint;
    //默认起飞高度为2[m]，起飞位置为坐标原点，偏航为0
    pos_setpoint.pose.position.x = 0;
    pos_setpoint.pose.position.y = 0;
    pos_setpoint.pose.position.z = 2;

    pos_setpoint.pose.orientation.w = 0;
    pos_setpoint.pose.orientation.x = 0;
    pos_setpoint.pose.orientation.y = 0;
    pos_setpoint.pose.orientation.z = 0;

    setpoint_raw_local_pub.publish(pos_setpoint);
}

void command_to_rotors::land()
{
    geometry_msgs::PoseStamped pos_setpoint;

    //默认降落高度为0[m]，起飞位置为坐标原点，偏航为0
    pos_setpoint.pose.position.x = 0;
    pos_setpoint.pose.position.y = 0;
    pos_setpoint.pose.position.z = 0;

    pos_setpoint.pose.orientation.w = 0;
    pos_setpoint.pose.orientation.x = 0;
    pos_setpoint.pose.orientation.y = 0;
    pos_setpoint.pose.orientation.z = 0;

    setpoint_raw_local_pub.publish(pos_setpoint);
}
/**************************************************
void command_to_rotors::loiter()
悬停模式利用setpoint实现，所以该函数删除
*************************************************/
void command_to_rotors::idle()
{
    geometry_msgs::PoseStamped pos_setpoint;

    //象征性的飞0.1m
    pos_setpoint.pose.position.x = 0;
    pos_setpoint.pose.position.y = 0;
    pos_setpoint.pose.position.z = 0.1;

    pos_setpoint.pose.orientation.w = 0;
    pos_setpoint.pose.orientation.x = 0;
    pos_setpoint.pose.orientation.y = 0;
    pos_setpoint.pose.orientation.z = 0;


    setpoint_raw_local_pub.publish(pos_setpoint);
}

//发送位置期望值至飞控（输入：期望xyz,期望yaw）
void command_to_rotors::send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp)
{
    geometry_msgs::PoseStamped pos_setpoint;
    //设置定点飞行的期望位置点
    Eigen::Vector3d roll_pitch_yaw = Eigen::Vector3d(0.0,0.0,yaw_sp);
    Eigen::Quaterniond orientation_desired;
    pos_setpoint.pose.position.x = pos_sp[0];
    pos_setpoint.pose.position.y = pos_sp[1];
    pos_setpoint.pose.position.z = pos_sp[2];
    orientation_desired=quaternion_from_rpy(roll_pitch_yaw);

    tf::quaternionEigenToMsg(orientation_desired,pos_setpoint.pose.orientation);
    // 检查飞控是否收到控制量
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_rotors<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "Pos_target [X Y Z] : " << pos_setpoint.pose.position.x << " [ m ] "<< pos_setpoint.pose.position.y<<" [ m ] "<<pos_setpoint.pose.position.z<<" [ m ] "<<endl;
    cout << "Pos_target [W X Y Z] : " << pos_setpoint.pose.orientation.z<< " [ 。] "<< pos_setpoint.pose.orientation.w<<" [ 。 ] "<<endl;
    cout <<">>>>>>>>>>>>>>>>>>only see orientation(z and w), other num is zero [can web search]<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    setpoint_raw_local_pub.publish(pos_setpoint);
}
#endif


